import math
import numpy as np
import logging
import os
import datetime
import time

# from datetime import now

logger = logging.getLogger(__name__)
pren = 20
PI = math.pi


# 别忘了把i重置


class LQR_controller:
    def __init__(self):
        self.lf = 0.71
        self.lr = 1.2-0.71
        self.m = 125.76
        self.L = 1.2
        self.Cf = -2020
        self.Cr = -2020
        self.i = 0  # 运行到的点位(匹配点index)
        self.x = 0
        self.y = 0
        self.head = 0
        self.v = 10
        self.vx = 0  # 近似为d_s
        self.timestamp = 0
        self.phi = 0
        self.ki = 0
        self.kd = 0.005
        self.kp = 0.18
        self.steer = 0
        self.pre_thr=0
        self.thr = 0
        self.brake = 0
        self.K = np.zeros((3000, 4))
        self.y_bias_past = 0
        self.h_bias_past = 0
        self.I_jilu = np.zeros(10)
        self.I_jilu = list(self.I_jilu)
        self.pre_e = 0
        self.tar_xlist = None
        self.tar_ylist = None
        self.tar_hlist = None
        self.tar_vlist = None
        self.tar_klist = None
        self.tar_tlist = None
        self.lqr_mode = 0    #
        self.repeat_flag = False
        self.norepeat_phi_list = []
        self.norepeat_t_list = []
        self.t_stamp = 0.1  # 线性差值时间步长
        self.pre_dphi = 0.0
        self.ed = 0
        self.d_ed = 0
        self.ephi = 0
        self.d_ephi = 0
        self.match_x = 0
        self.match_y = 0
        self.match_h = 0
        self.match_v = 0
        self.match_k = 0
        self.pre_pi_steer = 0
        self.u_fankui = 0
        self.u_qiankui = 0
        self.u = 0

    def update_ego_state(self, curx, cury, curhead, curv, curtime, repeat_flag):
        self.x = curx
        self.y = cury
        self.head = curhead
        self.phi = self.head
        self.v = curv
        self.timestamp = curtime
        self.repeat_flag = repeat_flag
        if not self.repeat_flag:  # 如果未重复
            self.norepeat_t_list.append(self.timestamp)
            self.norepeat_phi_list.append(self.phi)

    def update_trajectory(self, xlist, ylist, hlist, vlist, tlist):
        self.LQR_Init()  # 初始化K
        xlist_temp, ylist_temp, hlist_temp, vlist_temp, tlist_temp = self.preprocess_data(xlist, ylist, hlist, vlist,
                                                                                          tlist)
        klist_temp = self.cal_k(xlist_temp, ylist_temp)

        self.tar_xlist = xlist_temp
        self.tar_ylist = ylist_temp
        self.tar_hlist = hlist_temp
        self.tar_vlist = vlist_temp
        self.tar_klist = klist_temp
        self.tar_tlist = tlist_temp

        # 算曲率要在数据预处理之后，要不会出现非常奇怪的曲率
        # print("self.tar_klist=", self.tar_klist)
        for i in range(0, len(self.tar_vlist)):
            if self.tar_vlist[i] < 0.1:
                self.tar_vlist[i] = 0.1

        self.log_target_list(xlist, ylist, hlist, vlist, tlist)

    def preprocess_data(self, x, y, h, v, t):
        xtemp = [x[0]]
        ytemp = [y[0]]
        htemp = [h[0]]
        vtemp = [v[0]]
        ttemp = [t[0]]
        sum_d = 0
        for i in range(1, len(x)):  # 生成一系列有规律间隔的点
            sum_d += math.hypot(x[i] - x[i-1], y[i] - y[i-1])
            if sum_d > 0.1:
                xtemp.append(x[i])
                ytemp.append(y[i])
                htemp.append(h[i])
                vtemp.append(v[i])
                ttemp.append(t[i])
                sum_d = 0
        return xtemp, ytemp, htemp, vtemp, ttemp

    def linear_interpolation(self, x_temp, y_temp, h_temp, v_temp, k_temp, t_temp):  # 线性插值(没用到)
        new_t_temp = np.arange(t_temp[0], t_temp[-1], self.t_stamp)  # self.t_stamp 未知
        print("new_t_temp[-1]=", new_t_temp[-1])
        print("t_temp=[-1]", t_temp[-1])
        new_x_temp = []
        new_y_temp = []
        new_h_temp = []
        new_v_temp = []
        new_k_temp = []
        for time in new_t_temp:
            match_idx = 99999999
            for i in range(len(t_temp)-1):
                if (time >= t_temp[i]) and (time <= t_temp[i+1]):
                    match_idx = i
                    break
            print("index=", match_idx)
            ratio = (time-t_temp[match_idx])/(t_temp[match_idx+1]-t_temp[match_idx])
            x_linear = x_temp[match_idx]+ratio*(x_temp[match_idx+1]-x_temp[match_idx])
            y_linear = y_temp[match_idx]+ratio*(y_temp[match_idx+1]-y_temp[match_idx])
            v_linear = v_temp[match_idx]+ratio*(v_temp[match_idx+1]-v_temp[match_idx])
            k_linear = k_temp[match_idx]+ratio*(k_temp[match_idx+1]-k_temp[match_idx])
            h_linear = self.NormalizeAngle(h_temp[match_idx]+ratio*self.NormalizeAngle(h_temp[match_idx+1]-h_temp[match_idx]))
            new_x_temp.append(x_linear)
            new_y_temp.append(y_linear)
            new_v_temp.append(v_linear)
            new_k_temp.append(k_linear)
            new_h_temp.append(h_linear)
            print("new_v_temp", new_v_temp)
        return new_x_temp, new_y_temp, new_h_temp, new_v_temp, new_k_temp, new_t_temp

    def closepoint(self):  # pren为前瞻步数
        if self.i + pren >= len(self.tar_xlist):
            n = len(self.tar_xlist)-self.i
        else:
            n = pren
        bx = self.tar_xlist[self.i:self.i+n]
        by = self.tar_ylist[self.i:self.i+n]
        x0list = np.ones(len(bx))*self.x
        y0list = np.ones(len(by))*self.y
        dMat = np.square(bx-x0list)+np.square(by-y0list)
        self.i = np.where(dMat == np.min(dMat))[0][0] + self.i

    def NormalizeAngle(self, angle):  # 将所有弧度整合至-pi到pi
        while angle >= math.pi:
            angle -= 2 * math.pi
        while angle < - math.pi:
            angle += 2 * math.pi
        return angle

    def track(self):
        self.closepoint()  # 找匹配点
        if (self.i < len(self.tar_xlist)-1) and self.lqr_mode == 1:
            self.match_x = self.tar_xlist[self.i]
            self.match_y = self.tar_ylist[self.i]
            self.match_h = self.tar_hlist[self.i]
            self.match_v = self.tar_vlist[self.i]
            self.match_k = self.tar_klist[self.i]

            # 计算横向偏差，航向偏差，横向偏差变化率，航向偏差变化率
            # 差分计算d_phi(当前车辆横摆角速度)
            if len(self.norepeat_phi_list) <= 1:  # 前两帧
                d_phi = 0.0
            else:  # 从记录的未重复列表末尾位取差分值
                d_phi = (self.NormalizeAngle(self.norepeat_phi_list[-1] - self.norepeat_phi_list[-2])) / (self.norepeat_t_list[-1] - self.norepeat_t_list[-2])
                if abs(d_phi) >= abs(self.pre_dphi)*3:
                    d_phi=self.pre_dphi
                self.pre_dphi = d_phi
                # d_phi=0.0

            # 计算匹配点切向量法向量，匹配点与实际点位矢
            match_n = np.array([[-math.sin(self.match_h)], [math.cos(self.match_h)]])  # 匹配点法向向量  math库中的三角函数运算符输入是弧度值
            match_tao = np.array([[math.cos(self.match_h)], [math.sin(self.match_h)]])  # 匹配点切向向量
            x_minus_matchx = np.array([self.x - self.match_x, self.y - self.match_y])  # 匹配点与车辆实际位置向量

            # 计算横向偏差  车左偏为正，右偏为负
            ed = np.dot(x_minus_matchx, match_n)
            self.ed = ed[0]
            if self.ed >= 9999 or self.ed <= -9999:  # 如果横向误差过大，则直接停止循迹
                logger.info("ed out of side")
                self.lqr_mode = 0

            # 匹配点与投影点之间的弧长es
            es = np.dot(x_minus_matchx, match_tao)
            es = es[0]

            # 投影点航向theta_r
            theta_r = self.NormalizeAngle(self.match_h + self.match_k * es)

            # 计算航向偏差ephi
            self.ephi = self.NormalizeAngle(self.phi - theta_r)

            # 投影点在frenet坐标轴上位矢的导数s_dot
            d_s = self.v * math.cos(self.NormalizeAngle(self.phi - theta_r)) / (1 - self.match_k * self.ed)
            self.vx = d_s

            # 计算横向偏差变化率
            self.d_ed = self.v * math.sin(self.NormalizeAngle(self.phi - theta_r))

            # 计算航向偏差变化率d_ephi
            self.d_ephi = d_phi - self.match_k * d_s
            # 计算横向偏差，角度偏差，横向变化率，角度变化率。投影点heading近似认为是匹配点heading
            # 计算横向偏差
            # 召唤lqr控制函数
            bias_steer=0
            pi_steer = bias_steer+180 / PI * self.LQR_Control(self.ed, self.d_ed, self.ephi, self.d_ephi)  # 弧度转角度
            pi_steer = - pi_steer[0, ]  # pi_steer 数组的第一个元素取负值
            pi_steer = np.clip(pi_steer, self.pre_pi_steer-7, self.pre_pi_steer+7)
            self.pre_pi_steer = pi_steer
            max_steer = 20
            if pi_steer > max_steer:  # 转向限制
                pi_steer = max_steer
            else:
                if pi_steer < -max_steer:
                    pi_steer = -max_steer

            v_error = self.v - self.match_v
            a_zhengze = self.PID_control(v_error)

            if a_zhengze >= 0.5:
                a_zhengze = 0.5
            if a_zhengze <= -1:
                a_zhengze = -1
            if a_zhengze >= 0:
                thr = a_zhengze
                brake = 0
                logger.info(f"pid thr = {a_zhengze}")
            else:
                thr = 0
                brake = -a_zhengze
                logger.info(f"pid brk = {-a_zhengze}")

            self.steer = float(pi_steer)/max_steer
            if len(self.norepeat_phi_list) > 1:
                thr = np.clip(thr, self.pre_thr-0.01, self.pre_thr+0.01)
            self.thr = thr
            self.pre_thr=thr
            self.brake = brake
        else:
            logger.info("lqr_controller (out of track) brake = 0.95")
            self.lqr_mode = 0
            self.steer = 0
            self.thr = 0
            self.brake = 0.95

        # 停住之后初始化
        if self.lqr_mode == 0:
            logger.info("stop and initial brake = 0.8")
            self.steer = 0
            self.thr = 0
            self.brake = 0.8
            self.i = 0
            self.I_jilu = np.zeros(10)
            self.I_jilu = list(self.I_jilu)
            self.y_bias_past = 0
            self.h_bias_past = 0
            self.pre_e = 0
            self.timestamp = 0
            self.phi = 0
            self.pre_thr=0.0

        return self.lqr_mode

    def LQR_Init(self):
        self.K = np.loadtxt('./conf/K.txt')

    def LQR_Control(self, l_bias, d_l_bias, h_bias, d_h_bias):
        X = np.array([[l_bias], [d_l_bias], [h_bias], [d_h_bias]])
        xvhao = int(self.v * 100)
        if xvhao >= 3000:
            k = self.K[2999, :]
        else:
            k = self.K[xvhao, :]
        print("k=", k)
        print("X=", X)
        self.u_fankui = -np.dot(k, X)
        self.u_qiankui = self.tar_klist[self.i]*(self.L-self.lr*k[2]-(self.m*self.vx*self.vx/self.L)
                                        * (self.lr/self.Cf+self.lf*k[2]/self.Cr-self.lf/self.Cr))
        self.u = self.u_fankui + self.u_qiankui
        return self.u

    def PID_control(self, e):
        # e = e
        self.I_jilu.pop(0)  # I_jilu 是大小为10的list
        self.I_jilu.append(e)

        d_temp = (e-self.pre_e)/0.1
        self.pre_e = e

        thr_P = self.kp * e
        thr_I = self.ki * sum(self.I_jilu)
        thr_D = self.kd * d_temp

        u = thr_P + thr_I + thr_D
        u = -u
        return u
    def cal_k(self, x, y):  # 算曲率
        k = [0]
        for i in range(1, len(x) - 1):
            a_index = i - 1
            b_index = i
            c_index = i + 1
            d_a = math.sqrt(pow(x[b_index] - x[c_index], 2) + pow(y[b_index] - y[c_index], 2))
            d_b = math.sqrt(pow(x[a_index] - x[c_index], 2) + pow(y[a_index] - y[c_index], 2))
            d_c = math.sqrt(pow(x[b_index] - x[a_index], 2) + pow(y[b_index] - y[a_index], 2))
            if d_a < 0.01:
                d_a = 0.01
            if d_c < 0.01:
                d_c = 0.01
            cos = (d_a * d_a + d_c * d_c - d_b * d_b) / (2 * d_a * d_c)
            # 这里可能是精度上有差异，竟然能算出大于1的cos
            if (1 - cos * cos) < 0:
                k_temp = 0.01
            else:
                sin = math.sqrt(1 - cos * cos)
                k_temp = 2 * sin / d_b
            k.append(k_temp)
        k.append(0)
        return k

    def control_log(self):
        log_str1 = f"time={self.timestamp} x={self.x} y={self.y} phi={self.phi} v={self.v}"
        log_str2 = f"steer={self.steer} thr={self.thr} brk={self.brake}"
        logger.info(log_str1)
        logger.info(log_str2)

    def log_target_list(self, x_list, y_list, h_list, v_list, t_list):
        try:
            folder_name = "Target_list"
            folder_path = f"./{folder_name}"
            if not os.path.exists(folder_path):
                os.makedirs(folder_path)

            timestamp_str = time.strftime("%Y-%m-%d_%H_%M_%S")
            file_path = os.path.join(folder_path, f"target_list_{timestamp_str}.txt")

            count_list = list(range(len(x_list)))
            target_list = open(file_path, "w", encoding="UTF-8")
            for i in count_list:
                target_list.write(str(x_list[i]) + ' ' + str(y_list[i]) + ' ' + str(h_list[i]) + ' ' + str(v_list[i]) + ' ' + str(t_list[i]) + '\n')

            target_list.flush()
            target_list.close()
            logger.info("target list loaded")
        except:
            logger.info("target list loaded fail")


